    #include "unitBodies.h"
#include <iostream>
using namespace std;
//==========================================
//vectorPoint-------------------------------
//==========================================
//statics====================
VPool<VectorPoint> VectorPoint::pool(MAX_VB_STORAGE);
//constructors===============
VectorPoint::VectorPoint() {
    X = -1;
    Y = -1;
}
VectorPoint::VectorPoint(float x, float y) {
    X = x;
    Y = y;
}
//Acessor====================
float VectorPoint::distanceTo(VectorPoint* vp) {
    float smallX = X - vp->getX();
    float smallY = Y - vp->getY();
    return sqrt((smallX * smallX)) + ((smallY * smallY));
}
int VectorPoint::getX() {
    return X;
}
int VectorPoint::getY() {
    return Y;
}
bool VectorPoint::equals(float x, float y){
    if (X == x && Y == y) return true;
    else return false;
}

//Manips=====================
void VectorPoint::setPoint(float x, float y){
    X = x;
    Y = y;
}
VectorPoint* VectorPoint::dotProduct(VectorPoint* vpoint) {
    return VectorPoint::getCleanVP(X + vpoint->getX(), Y + vpoint->getY());
}
int VectorPoint::determinant(VectorPoint* vp) {
    return X * vp->Y - Y * vp->X;
}
float VectorPoint::magnitutde() {
    return sqrt(X * X + Y * Y);
}
void VectorPoint::normalize() {
    float mag = magnitutde();
    X /= mag;
    Y /= mag;
}
//Operators==================
VectorPoint VectorPoint::operator+(const VectorPoint vp) {
    return VectorPoint(vp.X + X, vp.Y + Y);
}
VectorPoint VectorPoint::operator+(VectorPoint* vp) {
    return VectorPoint(vp->X + this->X, vp->Y + this->Y);
}
VectorPoint VectorPoint::operator-(const VectorPoint vp) {
    return VectorPoint(vp.X - X, vp.Y - Y);
}
VectorPoint VectorPoint::operator-(VectorPoint* vp) {
    return VectorPoint(vp->X - this->X, vp->Y - this->Y);
}
VectorPoint VectorPoint::operator*(const float scalar) {
    return VectorPoint(X * scalar, Y * scalar);
}
bool VectorPoint::operator==(const VectorPoint& vp) {
    return (X == vp.X && Y == vp.Y);
}
void VectorPoint::add(VectorPoint* vp) {
    X += vp->X;
    Y += vp->Y;
}
void VectorPoint::mul(VectorPoint* vp) {
    X *= vp->X;
    Y *= vp->Y;
}
void VectorPoint::mul(float scalar) {
    X *= scalar;
    Y *= scalar;
}
void VectorPoint::div(VectorPoint* vp) {
    X /= vp->X;
    Y /= vp->Y;
}
void VectorPoint::div(float scalar) {
    X /= scalar;
    Y /= scalar;
}
//pool functions=============
VectorPoint* VectorPoint::getCleanVP() {
    return getCleanVP(0, 0);
}
VectorPoint* VectorPoint::getCleanVP(float x, float y) {
    VectorPoint* p = pool.getPooledItem();
    p->X = x;
    p->Y = y;
    return p;
}
void VectorPoint::recycle() {
    pool.recycleItem(this);
}
//==========================================
//vectorData--------------------------------
//==========================================
//statics====================
VPool<VectorBody> VectorBody::pool(MAX_VB_STORAGE);
//constrictors===============
VectorBody::VectorBody() {
    center = NULL;
    maxRadius = 0;
    colorR = colorG = colorB = 0;
}
VectorBody::VectorBody(vector<VectorPoint*> p) {
    center = NULL;
    for (int i = 0; i < p.size(); i++) {
        points.push_back(p[i]);
    }
    findCenter();
    findMaxRange();
}
VectorBody::VectorBody(list<VectorPoint*> p) {
    center = NULL;
    list<VectorPoint*>::iterator itr;
    for (itr = p.begin(); itr != p.end(); itr++) {
        points.push_back(*itr);
    }
}
VectorBody::VectorBody(VectorBody* p) {
    center = NULL;
    vector<VectorPoint*>::iterator itr;
    for (itr = p->getBegin(); itr != p->getEnd(); itr++) {
        points.push_back(*itr);
    }
}
VectorBody::~VectorBody() {
    if (center != NULL) center->recycle();
}
//color data=================
void VectorBody::sColor(float r, float g, float b) {
    if (r > 1) r = 1; //chack r
    if (r < 0) r = 0;
    if (g > 1) g = 1; //chack g
    if (g < 0) g = 0;
    if (b > 1) b = 1; //check b
    if (b < 0) b = 0;
    colorR = r;
    colorG = g;
    colorB = b;
}
void VectorBody::gColor(float* r, float* g, float* b) {
    *r = colorR;
    *g = colorG;
    *b = colorB;
}
//vertex data================
void VectorBody::moveVertex(float x, float y, float newX, float newY){
    VDIterator itr;
    for(itr = points.begin(); itr != points.end(); itr++){
        if((*itr)->equals(x, y)){
            (*itr)->setPoint(newX, newY);
        }
    }
}
void VectorBody::addVertex(float x, float y) {
    points.push_back(VectorPoint::getCleanVP(x, y));
    findCenter();
    int tempD = points[points.size() - 1]->distanceTo(center);
    if (tempD > maxRadius) {
        maxRadius = tempD;
    }
}
void VectorBody::removeVertex(float x, float y) {
    VectorPoint p = VectorPoint(x, y);
    VDIterator itr;
    for (itr = points.begin(); itr != points.end(); itr++) {
        if (p == **itr) {
            points.erase(itr);
            findMaxRange();
            break;
        }
    }
}
VectorPoint* VectorBody::getCenter() {
    if (center == NULL) {
        if (points.empty()) {
            return VectorPoint::getCleanVP(-1, -1);
        }
        findCenter();
    }
    return VectorPoint::getCleanVP(center->getX(), center->getY());
}
VDIterator VectorBody::getBegin() {
    return points.begin();
}
VDIterator VectorBody::getEnd() {
    return points.end();
}
//Collision==================
void VectorBody::setOnCollision(collisionHandler* ch) {
    chandler = ch;
}
void VectorBody::onCollision(VectorBody* collidesWith) {
    chandler->onCollision(collidesWith);
}
bool VectorBody::collidesWith(VectorBody* second) {
    return false;
}
/**
 * uses pnpolly by w randolf franklin
 * tests if the given point is contined
 *
 * @param x x pos to test
 * @param y y pos to test
 * @return if the polygon contains the point
 */
bool VectorBody::containsPoint(float x, float y) {
    bool inside = false;
    int i = 0, j = 0;
    for (i = 0, j = points.size() - 1; i < points.size(); j = i++) {
        //if ( ((verty[i]>testy) != (verty[j]>testy)) &&
        //(testx < (vertx[j]-vertx[i]) * (testy-verty[i]) / (verty[j]-verty[i]) + vertx[i])
        if (((points[i]->getY() > y) != (points[j]->getY() > y)) &&
                (x < (points[j]->getX() - points[i]->getX()) * (y - points[i]->getY()) / (points[j]->getY() - points[i]->getY()) + points[i]->getX())) {
            inside = !inside;
        }
    }
    return inside;
}
bool VectorBody::circleTest(VectorBody* second) {
    VectorPoint* c = getCenter();
    int centerDistance = second->getCenter()->distanceTo(c);
    if (centerDistance <= maxRadius || centerDistance <= second->maxRadius) {
        return true;
    }
    return false;
}
float VectorBody::findMaxRange() {
    VectorPoint* c = getCenter();
    float tempRadius;
    maxRadius = 0;
    for (int i = 0; i < points.size(); i++) {
        tempRadius = c->distanceTo(points[i]);
        if(tempRadius > maxRadius){
            maxRadius = tempRadius;
        }
    }
    c->recycle();
    return maxRadius;
}
void VectorBody::findCenter() {
    if (center != NULL) center->recycle();
    center = VectorPoint::getCleanVP(0, 0);
    for (int i = 0; i < points.size(); i++) {
        center->add(points[i]);
    }
    center->div(points.size());
}
//access=====================
float VectorBody::getMaxRadius(){
    if (maxRadius == 0) findMaxRange();
    return maxRadius;
}
//manipulators===============
void VectorBody::moveCenter(VectorPoint* vp) {
    if (center == NULL) findCenter();
    VectorPoint* translationVect = VectorPoint::getCleanVP();
    *translationVect = *getCenter() - *vp;
    cout << "translatoin: " << translationVect->getX() << " " << translationVect->getY() << endl;
    for (int i = 0; i < points.size(); i++) {
        (points[i])->add(translationVect);
    }
    center->add(vp);
}
void VectorBody::moveCenter(float x, float y){
    VectorPoint* p = VectorPoint::getCleanVP(x, y);
    moveCenter(p);
    p->recycle();
    p = NULL;
}
void VectorBody::slideCenter(VectorPoint* vp){
    if (center == NULL) findCenter();
    for (int i = 0; i < points.size(); i++) {
        (points[i])->add(vp);
    }
    center->add(vp);
}
void VectorBody::slideCenter(float x, float y){
    VectorPoint* p = VectorPoint::getCleanVP(x, y);
    slideCenter(p);
    p->recycle();
    p = NULL;

}
//pool functions=============
VectorBody* VectorBody::getCleanVB() {
    VectorBody* vb = pool.getPooledItem();
}
void VectorBody::recycle() {
    points.clear();
    center->recycle();
    pool.recycleItem(this);
}
//==========================================
//VDATA WORLD-------------------------------
//==========================================
//constructor---------------------
VDataWorld::VDataWorld() {
}
VDataWorld::~VDataWorld() {
}
//detection-----------------------
void VDataWorld::preformCollisions() {
}
void VDataWorld::preformCollisions(VectorBody* vd) {
}
bool VDataWorld::CheckForCollision(VectorBody* vd) {
    VDIterator iter;
}
//DATA MANAGMENT------------------
void VDataWorld::addVData(VectorBody* vd) {
    bodies.push_back(vd);
}
void VDataWorld::removeVData(VectorBody* vd) {
    bodies.remove(vd);
}
void VDataWorld::cleanWorld() {
    bodies.clear();
}
//ITERATORS-----------------------
VBIterator VDataWorld::begining() {
    return bodies.begin();
}
VBIterator VDataWorld::end() {
    return bodies.end();
}